'''
测试
'''
import json

def img2robot(x_camera, y_camera, m):
    """
    相机坐标通过仿射矩阵变换取得机器坐标
    :param x_camera: float
    :param y_camera: float
    :param m: list(list(float))  2*3
    :return: robot_x, robot_y # float float
    """
    # m = self.get_m(STC_points_camera, STC_points_robot)
    robot_x = (m[0][0] * x_camera) + (m[0][1] * y_camera) + m[0][2]
    robot_y = (m[1][0] * x_camera) + (m[1][1] * y_camera) + m[1][2]
    # robot =   (m[2][0] * x_camera) + (m[2][1] * y_camera) + m[2][2]
    # print(robot)
    # print(robot_x)
    # print(robot_y)
    return robot_x, robot_y


if __name__=="__main__":

    with open('9p_calib_cable_cam3.json', 'r') as f:
        calib_res_dict = json.load(f)

    matrix = calib_res_dict['Matrix']
    cam_jis = calib_res_dict['Cam_jis']
    arm_xys = calib_res_dict['Arm_xys']

    for ind, (j, i) in enumerate(cam_jis):
        x_camera, y_camera = j, i
        x_robot, y_robot = img2robot(x_camera, y_camera, matrix) # 像素坐标系（ji） 转 世界坐标系
        print(f'tru xy_robot  {arm_xys[ind][0]} {arm_xys[ind][1]}')
        print(f'pre xy_robot  {x_robot} {y_robot}\n')